#include <stdio.h>
#include <math.h>
#include <assert.h>

#define NLIGHTSAMP 250
#define NLIGHTSAMPBY2 125
#define NLIGHTSAMPBY4 63
#define ADCMIN 0
#define ADCMAX 1005

// 2pi/(NLIGHTSAMP/6)
#define DPHI120 0.15079644737231007

unsigned short buffer[256];
#define WAVEDATA(K) buffer[(K)+6]

unsigned char byteValue;
unsigned short uintValue;
float floatValue;

unsigned cycle,nzero;
float sink,cosk,beta0,beta1,beta2;
float alpha00,alpha01,alpha02,alpha11,alpha12,alpha22;

// low-pass filtering
unsigned char counter,lowPassM=1,lowPassN=1;
void lowPassFilter() {
    while(++counter) {
        buffer[counter] = lowPassM*buffer[counter-1] + lowPassN*buffer[counter];
    }
}

// lightingAnalysis makes the following math calls: 62*sin, 62*cos, 3*sqrt
void lightingAnalysis() {
    nzero = 0;
    alpha00 = NLIGHTSAMP;
    alpha11 = alpha22 = NLIGHTSAMPBY2;
    alpha01 = alpha02 = alpha12 = 0;
    beta0 = beta1 = beta2 = 0;
    for(byteValue = 0; byteValue < NLIGHTSAMPBY4; byteValue++) {
        if(byteValue == 0) {
            sink = 0;
            cosk = 1;
        }
        else {
            floatValue = DPHI120*byteValue;
            sink = sin(floatValue);
            cosk = cos(floatValue);
        }
        for(cycle = 0; cycle < 4; cycle++) {
            if((cycle%2) && (byteValue==0)) continue;
            switch(cycle) {
                case 0:
                uintValue = WAVEDATA(byteValue);
                break;
                case 1:
                uintValue = WAVEDATA(NLIGHTSAMPBY2-byteValue);
                break;
                case 2:
                uintValue = WAVEDATA(NLIGHTSAMPBY2+byteValue);
                break;
                case 3:
                uintValue = WAVEDATA(NLIGHTSAMP-byteValue);
                break;
            }
            if(uintValue > ADCMIN && uintValue < ADCMAX) {
                floatValue = uintValue;
                beta0 += floatValue;
                beta1 += floatValue*cosk;
                beta2 += floatValue*sink;
            }
            else {
                if(uintValue <= ADCMIN) nzero++;
                alpha00 -= 1;
                alpha01 -= cosk;
                alpha02 -= sink;
                alpha11 -= cosk*cosk;
                alpha12 -= sink*cosk;
                alpha22 -= sink*sink;
            }
            sink = -sink;
        }
    }
    //printf("%lf %lf %lf\n",alpha00,alpha01,alpha02);
    //printf("%lf %lf\n",alpha11,alpha12);
    //printf("%lf\n",alpha22);
    //printf("\n%lf %lf %lf\n",beta0,beta1,beta2);
    
    // Check for an almost singular matrix which signals an over/under-flow condition
    // in the sampling.
    if(alpha00 < 15.) {
        if(nzero > NLIGHTSAMPBY2) {
            // underflow
            beta0 = beta1 = 0;
        }
        else {
            // overflow
            beta0 = 999999.;
            beta1 = 0.;
        }
    }
    else {
        // Calculate the Cholesky decomposition of the symmetric postitive definite matrix alpha(i,j).
        // Replace each alpha(i,j) with the lower matrix element L(j,i).
        alpha00 = sqrt(alpha00);
        alpha01 /= alpha00;
        alpha02 /= alpha00;
        alpha11 = sqrt(alpha11-alpha01*alpha01);
        alpha12 = (alpha12-alpha01*alpha02)/alpha11;
        alpha22 -= alpha02*alpha02+alpha12*alpha12; // this is actually L(2,2)^2
    
        // Solve the linear equations alpha . x = beta for the vector of coeficients x.
        // Replace each beta(i) with x(i)
        beta0 /= alpha00;
        beta1 = (beta1 - alpha01*beta0)/alpha11;
        beta2 = (beta2 - alpha02*beta0 - alpha12*beta1)/alpha22;
        beta1 = (beta1 - alpha12*beta2)/alpha11;
        beta0 = (beta0 - alpha01*beta1 - alpha02*beta2)/alpha00;
    
        // beta0 is the mean lighting level.
        // Store the 120Hz peak amplitude is beta1:
        beta1 = sqrt(beta1*beta1 + beta2*beta2);
        //printf("\nA0 = %lf , A1 = %lf\n",beta0,beta1);
    }
}

double pi;
#define N120 (NLIGHTSAMP/6.)

void generate(double A0,double A1,double phi) {
    int k;
    double value;
    for(k = 0; k < 256; k++) {
        value = A0+A1*sin(2*pi*k/N120-phi);
        if(value <= ADCMIN) {
            buffer[k] = ADCMIN;
        }
        else if(value >= ADCMAX) {
            buffer[k] = ADCMAX;
        }
        else {
            buffer[k] = (unsigned short)round(value);
        }
    }
}

void dump(double scale) {
    int k;
    double scaled;
    printf("\n\n");
    for(k = 0; k < NLIGHTSAMP; k++) {
        scaled = WAVEDATA(k)/scale;
        printf("%lf\n",scaled);
    }
}

main() {
    pi = 4*atan(1.0);
    
    generate(3,5,0.321);
    dump(1);
    lowPassFilter();
    dump(lowPassN+lowPassM);
    return 0;
    lightingAnalysis();
    printf("%lf %lf\n",beta0,beta1);
    lowPassFilter();
    lightingAnalysis();
    printf("%lf %lf\n",beta0,beta1);
    return 0;
    
    // loop over A0 values
    int index;
    double A0,A1,phase = 0.123;
    for(index = 0; index <= 2800; index += 25) {
        A0 = index;
        A1 = 2*A0;
        generate(A0,A1,phase);
        lightingAnalysis();
        printf("%lf -> %lf , %lf -> %lf\n",A0,beta0,A1,beta1);
    }
}
